#include "my_hardware_interface/wheel_legged_robot_motor_socketcan.hpp"

#include <cmath>
#include <vector>

namespace my_hardware_interface
{
wheel_legged_robot_motor_socketcan::wheel_legged_robot_motor_socketcan()
{

}
wheel_legged_robot_motor_socketcan::~wheel_legged_robot_motor_socketcan()
{
    wheel_motor_shutdown();
    leg_motor_shutdown();
    close(socket_fd_);
}

bool wheel_legged_robot_motor_socketcan::socketcan_init(const char* ifname)
{
    // 创建socket
    if ((socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("Error while opening socket");
        return false;
    }
    // 设置CAN接口名称
    std::strcpy(ifr_.ifr_name, ifname);
    if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr_) < 0) {
        perror("Error getting interface index");
        close(socket_fd_);
        return false;
    }
    // 绑定socket到CAN接口
    addr_.can_family = AF_CAN;
    addr_.can_ifindex = ifr_.ifr_ifindex;
    if (bind(socket_fd_, (struct sockaddr *)&addr_, sizeof(addr_)) < 0) {
        perror("Error binding socket");
        close(socket_fd_);
        return false;
    }
    return true;
}

bool wheel_legged_robot_motor_socketcan::wheel_motor_start()
{
    struct can_frame sframe;
    struct can_frame rframe;
    sframe.can_id = wheel_motor_id_;
    sframe.can_dlc = 8;
    sframe.data[0] = 0x88U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    sframe.data[6] = 0x00U;
    sframe.data[7] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] == 0x88U)
    {
        return true;
    }
    else
    {
        perror("wheel motor start failed");
        return false;
    }
}

bool wheel_legged_robot_motor_socketcan::leg_motor_start()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_id = leg_motor_id_;
    // 设置控制模式为力矩控制
    sframe.can_dlc = 6;
    sframe.data[0] = 0x00U;
    sframe.data[1] = 0x4EU;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x01U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机力矩模式异常");
        return false;
    }

    // 设置控制源为不使用
    sframe.can_dlc = 6;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0x12U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机控制源异常");
        return false;
    }

    // 设置模拟量类型为内部使用
    sframe.can_dlc = 6;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0xFDU;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机模拟量为内部使用异常");
        return false;
    }

    // 设置模拟量初始值(设置为0)
    sframe.can_dlc = 6;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0xFEU;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机模拟量初始值异常");
        return false;
    }

    // 设置最大速度限制(设为250000count/s)
    sframe.can_dlc = 6;
    sframe.data[0] = 0x02U;
    sframe.data[1] = 0x04U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x03U;
    sframe.data[4] = 0xD0U;
    sframe.data[5] = 0x90U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机最大速度异常");
        return false;
    }

    // 电机使能
    sframe.can_dlc = 6;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x01U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("大腿电机使能异常");
        return false;
    }
    return true;
}

bool wheel_legged_robot_motor_socketcan::wheel_motor_shutdown()
{
    struct can_frame sframe;
    struct can_frame rframe;
    sframe.can_id = wheel_motor_id_;
    sframe.can_dlc = 8;
    sframe.data[0] = 0x80U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    sframe.data[6] = 0x00U;
    sframe.data[7] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] == 0x80U)
    {
        return true;
    }
    else
    {
        perror("wheel motor shutdown failed");
        return false;
    }
}

bool wheel_legged_robot_motor_socketcan::leg_motor_shutdown()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_dlc = 6;
    sframe.can_id = leg_motor_id_;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("大腿电机失能异常");
        return false;
    }
    return true;
}

void wheel_legged_robot_motor_socketcan::wheel_motor_effort_control(double effort)
{
    struct can_frame sframe;
    struct can_frame rframe;
    sframe.can_id = wheel_motor_id_;
    sframe.can_dlc = 8;
    // 根据多项式拟合,从扭矩(N*m)得出电流控制值
    // 多项式系数
    std::vector<double> p_iqcontrol = {-0.0126673649304591,		0.390247322840689,	-3.80068829566489,	12.8433455327651,		81.7666236370596,		-2.16406453027392};
    int16_t iqcontrol;
    if (effort > 0)
    {   
        //effort += 0.3;
        iqcontrol = (int16_t)(p_iqcontrol[0]*(pow(effort, 5)) + p_iqcontrol[1]*(pow(effort, 4)) + p_iqcontrol[2]*(pow(effort, 3)) + p_iqcontrol[3]*(pow(effort, 2)) + p_iqcontrol[4]*effort + p_iqcontrol[5]);
        // iqcontrol = (int16_t)((effort/0.665825)*2048.0/33.0);
    }
    else if(effort < 0)
    {
        effort = -effort;
        //effort += 0.3;
        double iq = (p_iqcontrol[0]*(pow(effort, 5)) + p_iqcontrol[1]*(pow(effort, 4)) + p_iqcontrol[2]*(pow(effort, 3)) + p_iqcontrol[3]*(pow(effort, 2)) + p_iqcontrol[4]*effort + p_iqcontrol[5]);
        iqcontrol = (int16_t)(-iq);
        // iqcontrol = (int16_t)((effort/0.665825)*2048.0/33.0);
    }
    else
    {
        iqcontrol = (int16_t)0;
    }
    sframe.data[0] = 0xA1U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = (uint8_t)(iqcontrol & 0xFFU);
    sframe.data[5] = (uint8_t)((iqcontrol >> 8) & 0xFFU);
    sframe.data[6] = 0x00U;
    sframe.data[7] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return ;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return ;
    }

}

void wheel_legged_robot_motor_socketcan::leg_motor_effort_control(double effort)
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_dlc = 6;
    sframe.can_id = leg_motor_id_;
    // 将目标扭矩转换为目标电流(mA)
    double iq;
    if (effort > 0.0)
    {
         iq = (129.7945*effort + 53.3847);
    }
    else if (effort < 0.0)
    {
         iq = (129.7945*effort - 53.3847);
    }
    else
    {
         iq = 0.0;
    }
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0xFEU;
    sframe.data[2] = (uint8_t)((int32_t)iq >> 24 & 0xFFU);
    sframe.data[3] = (uint8_t)((int32_t)iq >> 16 & 0xFFU);
    sframe.data[4] = (uint8_t)((int32_t)iq >>  8 & 0xFFU);
    sframe.data[5] = (uint8_t)((int32_t)iq & 0xFFU);
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return;
    }
}

double wheel_legged_robot_motor_socketcan::wheel_motor_angle_read()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_id = wheel_motor_id_;
    sframe.can_dlc = 8;
    sframe.data[0] = 0x92U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    sframe.data[6] = 0x00U;
    sframe.data[7] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return NAN;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return NAN;
    }
    if (rframe.data[0] == 0x92U)
    {
        // 判断数据的第7个字节的最高位作为符号位来补充第8个字节，组成64位
        int64_t motorangle;
        if (rframe.data[7] & 0x80U) // 最高位为1, 代表是负数
        {
            motorangle =      (static_cast<int64_t>(rframe.data[1])
                            + (static_cast<int64_t>(rframe.data[2]) << 8)
                            + (static_cast<int64_t>(rframe.data[3]) << 16)
                            + (static_cast<int64_t>(rframe.data[4]) << 24)
                            + (static_cast<int64_t>(rframe.data[5]) << 32)
                            + (static_cast<int64_t>(rframe.data[6]) << 40)
                            + (static_cast<int64_t>(rframe.data[7]) << 48)
                            + (static_cast<int64_t>(0xFFU) << 56)); // 0.01度/LSB
        }
        else
        {
            motorangle =      (static_cast<int64_t>(rframe.data[1])
                            + (static_cast<int64_t>(rframe.data[2]) << 8)
                            + (static_cast<int64_t>(rframe.data[3]) << 16)
                            + (static_cast<int64_t>(rframe.data[4]) << 24)
                            + (static_cast<int64_t>(rframe.data[5]) << 32)
                            + (static_cast<int64_t>(rframe.data[6]) << 40)
                            + (static_cast<int64_t>(rframe.data[7]) << 48)
                            + (static_cast<int64_t>(0x00U) << 56)); // 0.01度/LSB
        }
        // 将motorangle单位转换为rad
        double output_motorangle = (static_cast<double>(motorangle))*0.01*M_PI/180.0/reduction_ratio_;
        return output_motorangle;
    }
    else
    {
        perror("读取轮毂电机角度异常");
        return NAN;
    }
}

double wheel_legged_robot_motor_socketcan::wheel_motor_velocity_read()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_id = wheel_motor_id_;
    sframe.can_dlc = 8;
    sframe.data[0] = 0x9CU;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    sframe.data[6] = 0x00U;
    sframe.data[7] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return NAN;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return NAN;
    }
    if (rframe.data[0] == 0x9CU)
    {
        int16_t speed = static_cast<int16_t>(rframe.data[4] + (rframe.data[5] << 8)); // 1dps/LSB
        // 将speed单位转换为rad/s
        double output_speed = ((double)speed)*M_PI/180.0/reduction_ratio_;
        return output_speed;
    }
    else
    {
        perror("读取轮毂电机速度异常");
        return NAN;
    }
}

double wheel_legged_robot_motor_socketcan::leg_motor_position_read()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_dlc = 2;
    sframe.can_id = leg_motor_id_;
    sframe.data[0] = 0x00U;
    sframe.data[1] = 0x02U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return NAN;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return NAN;
    }
    // 返回的位置单位为count,转换为rad
    if (rframe.can_dlc != 1)
    {
        int32_t count = ((int32_t)(rframe.data[0]) << 24)
                      + ((int32_t)(rframe.data[1]) << 16)
                      + ((int32_t)(rframe.data[2]) <<  8)
                      + ((int32_t)(rframe.data[3]));
        double position = ((double)(count)/encorder_counts_)*2*M_PI;
        return position;
    }
    else
    {
        perror("读取大腿电机输出端位置异常");
        return NAN;
    }
}

double wheel_legged_robot_motor_socketcan::wheel_motor_iq_read()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_id = wheel_motor_id_;
    sframe.can_dlc = 8;
    sframe.data[0] = 0x9CU;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    sframe.data[6] = 0x00U;
    sframe.data[7] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return NAN;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return NAN;
    }
    if (rframe.data[0] == 0x9CU)
    {
        // 读取转矩电流
        int16_t Torque_iq = (static_cast<int16_t>(rframe.data[2]) + (static_cast<int16_t>(rframe.data[3]) << 8));
        double motor_iq = (double)Torque_iq*(33.0/2048.0);
        return motor_iq;
    }
    else
    {
        return NAN;
    }
}

double wheel_legged_robot_motor_socketcan::leg_motor_effort_read()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_dlc = 2;
    sframe.can_id = leg_motor_id_;
    sframe.data[0] = 0x02U;
    sframe.data[1] = 0x0DU;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return NAN;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return NAN;
    }
    if (rframe.can_dlc != 1)
    {
        int32_t effort = ((int32_t)(rframe.data[0]) << 24)
                       + ((int32_t)(rframe.data[1]) << 16)
                       + ((int32_t)(rframe.data[2]) <<  8)
                       + ((int32_t)(rframe.data[3]));
        double outputeffort = (double)effort/1000;
        return outputeffort;
    }
    else
    {
        perror("大腿电机实际扭矩读取异常");
        return NAN;
    }
}
}
